#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <queue>
#include "linkedlist.h"
#include "tracker.h"
#include "CarDb.h"

#include <time.h>

#define FRAMEGROUP_SIZE 5
#define FRAME_WAIT 27

using namespace std;

/*
bool isThisParkingSlot(CvPoint pnt) {
	return true;
}
*/
CRITICAL_SECTION list_cs;
static bool CRITICAL_SECTION_INT = false;
list<car> list_of_cars;

//#define CALEBRATE_X_Y
/*
 * #define CALEBRATE_X_Y	//Activate the helper to finding the X Y
 * #define HSV_DEBUG		//Activate the printf line to debug the HSV transform calculation
*/


/*OPTIMIZATION OPTIONS
 * Use the drected vector of the point that we found as the center of gravity.
 */



/*
 * Retrieves the maximum of 3 ints.
 */
int maxNum(uchar x, uchar y, uchar z) {
	int max =x;
	if (y > max) max = y;
	if (z > max) max = z;
	return max;
}

/*
 * Retrieves the minimum of 3 ints.
 */
int minNum(uchar x, uchar y, uchar z) {
	int min =x;
	if (y < min) min = y;
	if (z < min) min = z;
	return min;
}

/*
 * This function transform the frame from RGB to HSV.
 * Return Value: An array of int that holds the values for HSV.
 */
int* ConvertFromRgbToHsv(IplImage* img){
	int height = img->height;
	int width = img->width;
	int step = img->widthStep;
	int channels  = img->nChannels;
	uchar *data = (uchar *)img->imageData;
	uchar blue,green,red;
	int h=0,s,v,max,min;
	int* new_data = new int[height*width*channels];
	for (int y=0; y < height; ++y) {
		for (int x=0; x < width; ++x) {
			blue = data[y*step+x*channels+0];
			green = data[y*step+x*channels+1];
			red = data[y*step+x*channels+2] ;
			max = maxNum(blue,green,red);
			min = minNum(blue,green,red);
// V calculation
			v= max;
// S calculation
			if ( max == 0 ) s=0;
			else s= 1 - (min/max);
// H calculation
			if (max==min) max++;	//Because when max=min it's undefined
			if (max == red) h = ( (60*(green-blue)) / (max-min) ) % 360;
			else if (max == green) h = ( (60*(blue - red)) / (max - min) + 120 ) % 360;
			else if (max == blue) h = ( (60*(red-green)) / (max-min) + 240 ) % 360;
			if (h < 0) h+= 360;
////
			new_data[y*step+x*channels] = h;
			new_data[y*step+x*channels+1] = s;
			new_data[y*step+x*channels+2] = v;
#ifdef HSV_DEBUG
			printf ("yo, h=%d, s=%d, v=%d, red=%d, green=%d, blue=%d, max=%d min=%d", h, s, v, red, green, blue, max, min);
			cvWaitKey(0);
#endif
		}
	}
	return new_data;
}

/*
 * This function gets an image and finds the center of weight for the element that "p" is a point on it using an algorithm 
 * that uses spreading recursively from that point on.
 * Retun Value: a CvPoint that is the calculate center of weight.
 */

CvPoint calculateCenter(IplImage* img,int *new_data , car & car_to_check) {
	queue<CvPoint> que;
	int height = img->height;
	int width = img->width;
	int step = img->widthStep;
	CvPoint p = car_to_check.p;
	bool* used_arr = new bool[height*width];
	for (int i=0; i < (height*width); ++i) {
		used_arr[i]=false;	//initializing the array
	}
	int channels  = img->nChannels;
	uchar *data = (uchar *)img->imageData;
	
	uchar blue =car_to_check.blue ;
	uchar green =car_to_check.green ;
	uchar red = car_to_check.red ;
	int h =car_to_check.h ;
	int s =car_to_check.s ;
	int v =car_to_check.v ;
	
	que.push(p);
	CvPoint tmp;
	long int x_sum=0,y_sum=0,counter=0;
	while ( !que.empty() ){
		tmp = que.front();
		que.pop();
	
	if ( ((abs(new_data[tmp.y*step+tmp.x*channels] - h) <= 40) || 
		 ((abs(data[tmp.y*step+tmp.x*channels+0] - blue) <=  0) &&
		  (abs(data[tmp.y*step+tmp.x*channels+1] - green) <= 0) &&
		  (abs(data[tmp.y*step+tmp.x*channels+2] - red) <= 0) ) ) &&
		  (!(used_arr[tmp.y*width+tmp.x]))					) {

			used_arr[tmp.y*width+tmp.x] = true; 
			//data[tmp.y*step+tmp.x*channels+0] = 0; 
			//data[tmp.y*step+tmp.x*channels+1] = 0;
			//data[tmp.y*step+tmp.x*channels+2] = 255;
			x_sum+= tmp.x;
			y_sum+= tmp.y;
			counter++;
			if ((tmp.y < height-1) && (!(used_arr[(tmp.y+1)*width+tmp.x])) ) que.push(cvPoint(tmp.x, tmp.y+1));
			if ((tmp.y > 0) && (!(used_arr[(tmp.y-1)*width+tmp.x])) ) que.push(cvPoint(tmp.x, tmp.y-1));
			if ((tmp.x < width-1 ) && (!(used_arr[tmp.y*width+tmp.x+1])) ) que.push(cvPoint(tmp.x+1, tmp.y));
			if ((tmp.x > 0) && (!(used_arr[tmp.y*width+tmp.x-1])) ) que.push(cvPoint(tmp.x-1, tmp.y));
			}
	}
    delete[] used_arr;
	if ( counter ) {
	 x_sum = x_sum / counter;
	 y_sum = y_sum / counter;
	}else
	{
		x_sum = p.x;
		y_sum = p.y;
	}
	car_to_check.p = cvPoint(x_sum,y_sum);
    return  cvPoint(x_sum,y_sum);
}

bool isCarParked(CvPoint *PointArr){
	double center_x = (PointArr[0]).x;
	double center_y = (PointArr[0]).y;
	double dist=0;
	ParkinglotDB &Parking_DB=ParkinglotDB::instance();
	for(int i=1; i<NUMBER_OF_POINTS; ++i) {
		dist = sqrt( pow((PointArr[i].x - center_x),2) + pow((PointArr[i].y - center_y),2) );
		if (dist > OFFSET_RADIUS) return false;
	}
	return true;//(Parking_DB.isThisParkingSlot( (PointArr[0]).x, (PointArr[0]).y ) );
}


bool startTrackingCar(  CvCapture* capture) {
	int height, width, step, channels;
	int grabResult = 1;
	uchar *data;
	cvNamedWindow("YOOO HOOOO I'm THE KING!!!!!", 1);
	cvGrabFrame(capture);
	IplImage *firstFrame = cvRetrieveFrame(capture);
	cvShowImage("YOOO HOOOO I'm THE KING!!!!!", firstFrame);
	cvWaitKey(10);

	IplImage *img;
	int *new_data ;
	ParkinglotDB &DB=ParkinglotDB::instance();
#ifdef CALEBRATE_X_Y
		if ( cvGrabFrame(capture) == 0 ) return false;
		img =  cvRetrieveFrame(capture);  // retrieve the captured frame
		list_of_cars.GotoHead();
		while ( list_of_cars.CheckCurr() ){
			cvCircle(img, list_of_cars.GetCurrData().p , 5, cvScalar(0,255,0), 1);
			list_of_cars.GotoNext();
		}
		cvShowImage("YOOO HOOOO I'm THE KING!!!!!", img);
		cvWaitKey(500000000);
#endif
	time_t startTime = time(NULL);
	int frameCount = 0;
	int frameGroupCount = 1;
	while ( grabResult ) { 
		++frameCount;
		if( !(frameCount < FRAMEGROUP_SIZE) ){
			time_t currentTime = time(NULL);
			while( (currentTime-startTime) < frameGroupCount/2 ){
				currentTime = time(NULL);
			}
			++frameGroupCount;
			frameCount = 0;
		}
		if( frameGroupCount < FRAME_WAIT ){
			cvShowImage("YOOO HOOOO I'm THE KING!!!!!", firstFrame);
			cvWaitKey(10);			
			continue;
		}
		grabResult = cvGrabFrame(capture);
		//else if ( frameGroupCount == FRAME_WAIT ) cvNamedWindow("YOOO HOOOO I'm THE KING!!!!!", 1);
		//EnterCriticalSection( &list_cs );		// protecting the list
		img =  cvRetrieveFrame(capture);  // retrieve the captured frame
		height = img->height;
		width = img->width;
		step = img->widthStep;
		channels  = img->nChannels;
		data = (uchar *)img->imageData;
		new_data = ConvertFromRgbToHsv(img);
		list_of_cars.GotoHead();
	    cvRectangle(img, cvPoint(85,140) ,cvPoint(0,0) , cvScalar(0,0,255));
		while ( list_of_cars.CheckCurr() ){
			if (list_of_cars.GetCurrData().first_time) {
				list_of_cars.GetCurrData().blue = data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+0];
				list_of_cars.GetCurrData().green = data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+1];
				list_of_cars.GetCurrData().red = data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+2] ;
				list_of_cars.GetCurrData().h = new_data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+0];
				list_of_cars.GetCurrData().s = new_data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+1];
				list_of_cars.GetCurrData().v = new_data[list_of_cars.GetCurrData().p.y*step+list_of_cars.GetCurrData().p.x*channels+2] ;
				list_of_cars.GetCurrData().first_time= false;
			}	
			calculateCenter(img, new_data , list_of_cars.GetCurrData() );

			list_of_cars.GetCurrData().PointArr[list_of_cars.GetCurrData().IndexToPointArr++]=list_of_cars.GetCurrData().p;
		    if (list_of_cars.GetCurrData().IndexToPointArr == NUMBER_OF_POINTS) {
				list_of_cars.GetCurrData().check_park = true;
				list_of_cars.GetCurrData().IndexToPointArr = 0;
			}
			if ( (! list_of_cars.GetCurrData().is_park ) && isCarParked(list_of_cars.GetCurrData().PointArr) ) {
				DB.CarParked(list_of_cars.GetCurrData().licensePlate, list_of_cars.GetCurrData().p.x, list_of_cars.GetCurrData().p.y);
				printf("car %d parked \n",list_of_cars.GetCurrData().licensePlate);
				list_of_cars.GetCurrData().is_park = true;
			}
			if (  list_of_cars.GetCurrData().is_park  && (!isCarParked(list_of_cars.GetCurrData().PointArr)) ) {
				DB.CarUnparked(list_of_cars.GetCurrData().p.x, list_of_cars.GetCurrData().p.y);
				printf("car %d unparked \n",list_of_cars.GetCurrData().licensePlate);
				list_of_cars.GetCurrData().is_park = false;
			}
			cvCircle(img, list_of_cars.GetCurrData().p , 5, cvScalar(0,255,0), 1);
			list_of_cars.GotoNext();
		}
		delete[] new_data;
		cvShowImage("YOOO HOOOO I'm THE KING!!!!!", img);
		//LeaveCriticalSection( &list_cs );
		cvWaitKey(10);
		//cout << "nir" << endl;
	}
	//cvWaitKey(100);
	cvDestroyWindow("YOOO HOOOO I'm THE KING!!!!!");
	return true;
}


void initParkinglot( void *captureTop){		// == main_top
	if ( !CRITICAL_SECTION_INT ) 
		InitializeCriticalSection( &list_cs );	// will guard the linked list
    CvCapture* capture = (CvCapture *) captureTop;
	/*
	int movie=111,x,y;
	
	if (movie == 0 ) { 
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\36.avi");
		x=24; y=103;
		//carArrived( cvPoint(x,y) , 1 );
		x= 210; y = 130;
		//carArrived( cvPoint(x,y) , 2 );
	}
	if (movie == 1 ) {
		capture = cvCaptureFromAVI("S:\\flycap4.avi");
		x=950; y=20;
	}
	if (movie == 2 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\35.avi");
		x=275; y=130;
	}
	if (movie == 3 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\40.avi");
		x=150; y=96;
	}
	if (movie == 4 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\parking_lot~1.avi");
		//x=150; y=106;
	}
	if (movie == 5 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\demo2.avi");
		x=24; y=103;
		//carArrived( cvPoint(x,y) , 1 );
	}
	if (movie == 4 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\38.avi");
		x=220; y=136;
	}
	if (movie == 4 ) {
		capture = cvCaptureFromAVI("C:\\Documents and Settings\\snirbo\\Desktop\\39.avi");
		x=145; y=123;
	}*/
    startTrackingCar(capture);
	cvReleaseCapture(&capture);
}

bool carArrived( CvPoint center, int lp )
{
	//return true;
	//if ( !CRITICAL_SECTION_INT ) 
	      //InitializeCriticalSection( &list_cs );	// will guard the linked list
	car *tmp = new car;
	tmp->p= center;
	tmp->licensePlate = lp;
	//EnterCriticalSection( &list_cs );		// protecting the list
	bool res = list_of_cars.Insert( *tmp );
	//LeaveCriticalSection( &list_cs );
	return res;		
}


